#pragma config(Sensor, dgtl1,  ArmEncoderLeft,      sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  ArmEncoderRight,     sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseEncoderLeft,     sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  BaseEncoderRight,    sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  ProgramSwitch,       sensorTouch)
#pragma config(Motor,  port1,           LeftBase,      tmotorNormal, openLoop)
#pragma config(Motor,  port2,           LeftArm,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           LeftVacume,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           RightVacume,   tmotorNormal, openLoop)
#pragma config(Motor,  port8,           LeftTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           RightTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          RightBase,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard         !!*//

task antistallleft()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  SensorValue(BaseEncoderLeft) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[LeftBase] > 20 || motor[LeftBase] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[LeftBase] > 20 || motor[LeftBase] < -20)
        {
        quadPrev = SensorValue(BaseEncoderLeft); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - SensorValue(BaseEncoderLeft)) < (abs(motor[LeftBase]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[LeftBase] = 0; // stops motor
          ClearTimer(T1);
          hogCPU(); //stops task main from executing until the release button is pressed
          while(time1[T1] < 1000)
            { //this button releases the motor
            motor[LeftBase] = 0; //stops the motor which is not running correctly
            //insert all user controls except for the motor that is being stopped
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}

task antistallright()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  SensorValue(BaseEncoderRight) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[RightBase] > 20 || motor[RightBase] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[RightBase] > 20 || motor[RightBase] < -20)
        {
        quadPrev = SensorValue(BaseEncoderRight); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - SensorValue(BaseEncoderRight)) < (abs(motor[RightBase]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[RightBase] = 0; // stops motor
          ClearTimer(T1);
          hogCPU(); //stops task main from executing until the release button is pressed
          while(time1[T1] < 1000)
            { //this button releases the motor
            motor[RightBase] = 0; //stops the motor which is not running correctly
            //insert all user controls except for the motor that is being stopped
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}

task main()
{
   StartTask(antistallleft);
   StartTask(antistallright);
   while(true)
     {
       motor[LeftBase] = vexRT[Ch1];
       wait1Msec(1);//needed for multitasking to work correctly
     }
}
